/*
 * @Author: LiZhiJian
 * @Description: 
 * @Date: 2025-10-29 13:00:58
 * @LastEditTime: 2025-10-29 16:01:28
 * @Email: LiZhiJian8552@163.com
 */
#include "movement.h"
#include "servo.h"
#include "pwm.h"
#include "delay.h"
#include "stdlib.h"

int i,j;
// 用于规定改变动作的时间间隔
int movedelay=150;
uint8_t angle_1, angle_2, angle_3, angle_4;
// 站立
void move_stand(void){
    // 四个舵机都是90度
    Servo_SetAngle1(90);
    Servo_SetAngle2(90);
    Servo_SetAngle3(90);
    Servo_SetAngle4(90);
    Delay(500);
}
void move_forward(void){
    // n由于标记走路的步数
    int n=3;
    
    for(int i=0;i<n;i++){

        Servo_SetAngle1(135);
        Servo_SetAngle4(45);
        Delay(movedelay);
        

        Servo_SetAngle2(45);
        Servo_SetAngle3(135);
        Delay(movedelay);

        // 立正
        Servo_SetAngle1(90);
        Servo_SetAngle4(90);
        Delay(movedelay);
        
        Servo_SetAngle2(90);
        Servo_SetAngle3(90);
        Delay(movedelay);
    }
}
// 后退
void move_behind(void){
    int n=3;
    for(int i=0;i<n;i++){
        Servo_SetAngle1(45);
        Servo_SetAngle4(135);
        Delay(movedelay);
            
        Servo_SetAngle2(135);
        Servo_SetAngle3(45);
        Delay(movedelay);
            
        Servo_SetAngle1(90);
        Servo_SetAngle4(90);
        Delay(movedelay);
        
        Servo_SetAngle2(90);
        Servo_SetAngle3(90);
        Delay(movedelay);
    }
}
// 左转
void move_right(void){
    Servo_SetAngle1(45);
	Servo_SetAngle4(45);
	Delay(movedelay);
	
	Servo_SetAngle2(135);
	Servo_SetAngle3(135);
	Delay(movedelay);
	
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
	Delay(movedelay);
	
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Delay(movedelay);
}
// TODO 右转？
void move_right_hw(void){
    Servo_SetAngle1(45);
	Servo_SetAngle4(45);
	Delay(movedelay);
	
	Servo_SetAngle2(135);
	Servo_SetAngle3(135);
	Delay(movedelay);
	
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
//	Delay(movedelay);
	
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Delay(movedelay);
}
// 左转
void move_left(void){
    Servo_SetAngle2(135);
	Servo_SetAngle3(135);
	Delay(movedelay);
	
	Servo_SetAngle1(45);
	Servo_SetAngle4(45);
	Delay(movedelay);
	
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Delay(movedelay);
	
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
	Delay(movedelay);
}
void move_left_hw(void){
    Servo_SetAngle2(135);
	Servo_SetAngle3(135);
	Delay(movedelay);
	
	Servo_SetAngle1(45);
	Servo_SetAngle4(45);
	Delay(movedelay);
	
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
//	Delay(movedelay);
	
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
	Delay(movedelay);
}
// 打招呼
void move_hello(void){
    // 前后晃动
    for(i=0;i<20;i++){
        Servo_SetAngle1(90-i);
        Servo_SetAngle2(90+i);
        // TODO ?
        // Servo_SetAngle3(90+i);
        Servo_SetAngle4(90-i);
        Delay(4);
    }
    for(i=0;i<60;i++){
        Servo_SetAngle2(110+i);
        Servo_SetAngle4(70-i);
        Delay(4);
    }
    // 右前足缓慢抬起
    for(i=0;i<60;i++){
        Servo_SetAngle1(70+i);
        Delay(4);
    }
    Delay(50);
    //下面是摇两次右前足
	Servo_SetAngle1(180);
	Delay(500);
	Servo_SetAngle1(130);
	Delay(500);
	Servo_SetAngle1(180);
	Delay(500);
	Servo_SetAngle1(130);
	Delay(500);
	Servo_SetAngle1(70);
	Delay(500);
}
// 前后摇晃
void move_shake_qianhou(void){
    Servo_SetAngle2(135);
	Servo_SetAngle1(135);
	Servo_SetAngle3(45);
	Servo_SetAngle4(45);
	Delay(150);
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
	Delay(150);
	Servo_SetAngle2(45);
	Servo_SetAngle1(45);
	Servo_SetAngle3(135);
	Servo_SetAngle4(135);
	Delay(150);
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
	Delay(150);
}
// 左右摇晃
void move_shake_zuoyou(void){
    Servo_SetAngle1(135);
	Servo_SetAngle2(135);
	Delay(movedelay);
	Servo_SetAngle3(135);
	Servo_SetAngle4(135);
	Delay(movedelay);
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Delay(movedelay);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
	Delay(movedelay);
	Servo_SetAngle3(45);
	Servo_SetAngle4(45);
	Delay(movedelay);
	Servo_SetAngle1(45);
	Servo_SetAngle2(45);
	Delay(movedelay);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
	Delay(movedelay);
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Delay(movedelay);
}
void move_dance(void){
    Servo_SetAngle1(135);
	Servo_SetAngle2(135);
	Delay(movedelay);
	Servo_SetAngle3(135);
	Servo_SetAngle4(135);
	Delay(movedelay);
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Delay(movedelay);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
	Delay(movedelay);
	
	Servo_SetAngle2(135);
	Servo_SetAngle1(135);
	Servo_SetAngle3(45);
	Servo_SetAngle4(45);
	Delay(150);
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
	Delay(150);
	
	
	Servo_SetAngle3(45);
	Servo_SetAngle4(45);
	Delay(movedelay);
	Servo_SetAngle1(45);
	Servo_SetAngle2(45);
	Delay(movedelay);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
	Delay(movedelay);
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Delay(movedelay);
	
	Servo_SetAngle2(45);
	Servo_SetAngle1(45);
	Servo_SetAngle3(135);
	Servo_SetAngle4(135);
	Delay(150);
	Servo_SetAngle1(90);
	Servo_SetAngle2(90);
	Servo_SetAngle3(90);
	Servo_SetAngle4(90);
	Delay(150);
}
void move_butt_up(void){

}
// 抬头
void move_head_up(void){
    for(i=0;i<20;i++){
		Servo_SetAngle1(90-i);
		Servo_SetAngle3(90+i);
		Delay(10);
	}
	for(i=0;i<65;i++){
		Servo_SetAngle2(90+i);
		Servo_SetAngle4(90-i);
		Delay(10);
	}
}
void move(uint16_t set1,uint16_t set2,uint16_t set3,uint16_t set4,uint16_t speed){
    angle_1 = Servo_GetAngle1();
	angle_2 = Servo_GetAngle2();
	angle_3 = Servo_GetAngle3();
	angle_4 = Servo_GetAngle4();
	while (angle_1 != set1 || angle_3 != set3 || angle_2 != set2 || angle_4 != set4) {
		// 当前位置大于目标，减小角度
        if (angle_1 > set1) {
			--angle_1;
			Servo_SetAngle1(angle_1);
		} else if (angle_1 < set1) {    // 当前位置小于目标，增大角度
			++angle_1;
			Servo_SetAngle1(angle_1);
		}
		//
		if (angle_3 > set3) {
			--angle_3;
			Servo_SetAngle3(angle_3);
		} else if (angle_3 < set3) {
			++angle_3;
			Servo_SetAngle3(angle_3);
		}
		//
		if (angle_2 > set2) {
			--angle_2;
			Servo_SetAngle2(angle_2);
		} else if (angle_2 < set2) {
			++angle_2;
			Servo_SetAngle2(angle_2);
		}
		//
		if (angle_4 > set4) {
			--angle_4;
			Servo_SetAngle4(angle_4);
		} else if (angle_4 < set4) {
			++angle_4;
			Servo_SetAngle4(angle_4);
		}
		Delay(1000/speed);
	}
}
// 缓慢起身
void move_slow_stand(uint8_t previous_mode){
    // previous_mode用于表示是否已经起身
    if (previous_mode == '0')
		return;
	angle_1 = Servo_GetAngle1();
	angle_2 = Servo_GetAngle2();
	angle_3 = Servo_GetAngle3();
	angle_4 = Servo_GetAngle4();
	while (angle_1 != 90 || angle_3 != 90 || angle_2 != 90 || angle_4 != 90) {
		if (angle_1 > 90) {
			--angle_1;
			Servo_SetAngle1(angle_1);
		} else if (angle_1 < 90) {
			++angle_1;
			Servo_SetAngle1(angle_1);
		}
		//
		if (angle_3 > 90) {
			--angle_3;
			Servo_SetAngle3(angle_3);
		} else if (angle_3 < 90) {
			++angle_3;
			Servo_SetAngle3(angle_3);
		}
		//
		if (angle_2 > 90) {
			--angle_2;
			Servo_SetAngle2(angle_2);
		} else if (angle_2 < 90) {
			++angle_2;
			Servo_SetAngle2(angle_2);
		}
		//
		if (angle_4 > 90) {
			--angle_4;
			Servo_SetAngle4(angle_4);
		} else if (angle_4 < 90) {
			++angle_4;
			Servo_SetAngle4(angle_4);
		}
		Delay(10);
	}
}
// 坐下擦脸
void move_stretch(void){
    for(i=0;i<65;i++){//后两足缓慢前抬
        Servo_SetAngle2(90+i);
        Servo_SetAngle4(90-i);
        Delay(5);
	}
    for(i=0;i<20;i++){
        Servo_SetAngle1(90-i);
        Servo_SetAngle3(90+i);
        Delay(5);
	}
	Delay(1000);
	for(i=0;i<60;i++){//右前足缓慢抬起
		Servo_SetAngle1(70+i);
		Delay(4);
	}
	Delay(1000);
	//下面是摇两次右前足
	Servo_SetAngle1(180);
	Delay(500);
	Servo_SetAngle1(130);
	Delay(500);
	Servo_SetAngle1(180);
	Delay(500);
	Servo_SetAngle1(130);
	Delay(500);
	Servo_SetAngle1(70);
	Delay(5);//1:70;2:155;3:110;4:25
}
// 交替抬手
void move_two_hands(void){
    Servo_SetAngle3(20);
	Servo_SetAngle2(20);
	Delay(200);
	Servo_SetAngle3(90);
	Servo_SetAngle2(90);
	Delay(200);
	Servo_SetAngle1(160);
	Servo_SetAngle4(160);
	Delay(200);
	Servo_SetAngle1(90);
	Servo_SetAngle4(90);
	Delay(200);
}
void lan_yao(void){
    for(i=0;i<75;i++){
        Servo_SetAngle1(90+i);
        Servo_SetAngle3(90-i);
        Servo_SetAngle2(90+i/2);
        Servo_SetAngle4(90-i/2);
        Delay(5);
	}
	Delay(movedelay*50);
	for(i=0;i<75;i++){
        Servo_SetAngle1(165-i);
        Servo_SetAngle3(15+i);
        Servo_SetAngle2(127-i/2);
        Servo_SetAngle4(53+i/2);
        Delay(5);
	}
	Delay(movedelay);
}
// 趴下睡觉
void move_sleep_p(void){
    for(i=0;i<75;i++){
		Servo_SetAngle1(90+i);
		Servo_SetAngle3(90-i);
		Delay(10);
	}
	for(i=0;i<75;i++){
		Servo_SetAngle4(90+i);
		Servo_SetAngle2(90-i);
		Delay(10);
	}
}
// 卧下睡觉
void move_sleep_w(void){
    for(i=0;i<75;i++){
		Servo_SetAngle3(90+i);
		Servo_SetAngle1(90-i);
		Delay(10);
	}
	for(i=0;i<75;i++){
		Servo_SetAngle2(90+i);
		Servo_SetAngle4(90-i);
		Delay(10);
	}
}

